工具重力补偿接口说明

[TOC]

基于末端传感器的重力补偿基本流程和接口如下

1. 建立机器人算法接口实例

    /**
     * @brief 初始化算法库, 生成机器人模型
     * @param urdfModel: urdf模型, 以字符串形式描述
     * @param srdfModel: srdf模型, 以字符串形式描述
     * @return < 0, 表示初始化失败
     */
    ARAL_API_BASIC(1.0) int rlInitiateRobotModel(const std::string& urdfModel, const std::string& srdfModel) = 0;

    /**
     * @brief 初始化算法库, 设置机械臂模型及配置参数信息(算法会自动加载对应的文件)
     * @param robot_name: 机械臂名称
     * @param path: urdf, meshes等资源文件夹所在的路径(如./aral_export/aubo_description/)
     */
    ARAL_API_BASIC(1.0) int rlInitiateRobotModelFromFiles(const std::string& robot_name, const std::string& path) = 0;

2. 设置传感器相对于法兰坐标系的位姿

    /**
     * @brief 设置末端传感器坐标系在机器人法兰坐标系下的位姿
     * @param pose: 位置和姿态数组信息
     */
    ARAL_API_COMMON(1.0) void mdlSetEndSensorPoseInFlange(const RLPose& pose) = 0;

3. 设置 tcp 相对于法兰坐标系的位姿

    /**
     * @brief 设置工具坐标系的位姿
     *      1. 如果是末端工具，则为 tcp 相对于法兰坐标系的位姿
     *      2. 如果是远端工具，则为 tcp 相对于基坐标系的位姿, 需要先调用　setRemoteToolEnable　来使能远端工具
     * @param pose: 位置和姿态数组信息
     */
    ARAL_API_COMMON(1.0) void mdlSetToolPose(const RLPose& pose) = 0;

4. 标定传感器（得到工具在传感器坐标系的质量和质心）

    /**
     * @brief 根据末端 6 维力/力矩传感器的信息标定工具和传感器的属性( 3 点法), 只能标定工具的质量和质心
     * @param T_b_f: 输入多种不同构型对应的法兰在基坐标系的描述, 这几种构型的姿态差异越大越好（个数应不小于 3）
     * @param mesureData: 在对应构型下测得的传感器的原始数据
     * @param res: 标定结果(见参考文档)
     * @return 返回值 < 0, 表示计算失败
     */
    ARAL_API_COMMON(1.0) int calibToolDynamicParameterWithFTSensor(const std::vector<RLPose>& T_b_f, const std::vector<RLWrench>& mesureData, FtSensorCalibResult& res) = 0;

5.设置负载在传感器坐标系下的质量和质心

    /**
     * @brief 设置负载的动力学参数, 参考坐标系为末端法兰坐标系
     * @param inertial: 负载的动力学参数结构体
     */
    ARAL_API_COMMON(1.0) void mdlSetLoadDynamicParameterInFlange(const RLInertia& inertia) = 0;

说明：负载包括连接在传感器末端的工具和工件，可以采用以下接口将负载合并。

    /**
     * @brief 计算两个刚体惯性张量的和
     * @param body_a: 刚体 a 的惯性张量在参考坐标系 c 中的描述；
     * @param body_b: 刚体 b 的惯性张量在参考坐标系 c 中的描述；
     * @return  body_a + body_b 的转动惯量在参考坐标系 c 中的描述
     */
    ARAL_API_BASIC(1.0) RLInertia kdAddInertia(const RLInertia& body_a, const RLInertia& body_b)const = 0;

6. 得到在 tcp 处产生的外力

    /**
     * @brief 根据末端传感器数据计算得到作用在 TCP 末端 RLWrench (在 TCP 坐标系下描述, 重力补偿工具)
     * @param ft_sensor: 传感器数值
     * @param joint_pos: 机械臂关节位置
     * @param joint_vel: 机械臂关节速度
     * @param joint_acc: 机械臂关节加速度
     * @return: 作用在 TCP 末端 RLWrench (在 TCP 坐标系下描述)
     */
    ARAL_API_COMMON(1.0) RLWrench kdCalTCPWrenchFromEndSensor(const RLWrench& ft_sensor, const RLJntArray& joint_pos, const RLJntArray& joint_vel, const RLJntArray& joint_acc)const = 0;

说明：

1. 如需计算补偿负载重力后传感器处的力和力矩，调用以下接口进行变换；
2. 如果只补偿重力，将 joint_vel，joint_acc 设为0；
3. 传入的传感器数据为 去掉传感器偏置之后的值；ft_sensor = ft_sensor - offset

    /**
     * @brief 改变　RLWrench　的作用点和参考坐标系
     * @param ft_in_a: 输入作用在 a 点的 RLWrench 在 a 坐标系描述
     * @param F_b_a: a 相对于 b 的位姿
     * @return: 输出作用在 b 点的 RLWrench 在 b 坐标系描述
     */
    ARAL_API_BASIC(1.0) RLWrench kdChangeWrenchFrame(const RLWrench& ft_in_a, const RLPose& F_b_a)const = 0;

调用示例

见测试用例

SUITE(SUITE_ARAL_CAL_TOOL_GRAVITY_COMPENSATION_FLOW)
{
    TEST_FIXTURE(AuboRobotInterface, testtoolGravityCompensation)
    {
        //1. 初始化机器人实例
        Setup("aubo_i5");

        //2. 设置传感器相对于法兰坐标系的位姿
        interface::RLPose Tfs = {0.01, 0.02, 0.03, M_PI/2, 0, 0};
        robot->mdlSetEndSensorPoseInFlange(Tfs);

        //3. 设置 tcp 相对于法兰坐标系的位姿
        interface::RLPose Tft = {0.05, 0.01, 0.03, M_PI/2, 0, 0};
        robot->mdlSetToolPose(Tft);

        //4. 标定传感器（得到工具在传感器坐标系的质量和质心，该方法不能得到惯量）
        interface::FtSensorCalibResult res;

        std::vector<interface::RLJntArray> jointAngle { {0.144058 ,  -0.380091 ,  1.98637 ,  1.54486 ,  0.600783 ,  0.190936},
                                                       {-6.12349e-06 ,  -0.261797 ,  1.74532 ,  0.436327 ,  1.57079 ,  0},
                                                       {-6.12349e-06 ,  -0.462311 ,  1.33625 ,  -0.59648 ,  1.57079 ,  0} };

        std::vector<interface::RLWrench> mesureData{ {-0.981 ,  0 ,  -0.8829 ,  -1.6677 ,  -0.4905 ,  0.4905},
                                                    {-0.1962 ,  -1.962 ,  0.0981 ,  -2.4525 ,  -0.6867 ,  0.5886},
                                                    {-0.2943 ,  -3.3354 ,  -0.3924 ,  -2.8449 ,  -0.6867 ,  0.4905} };

        std::vector<interface::RLPose> T_b_f_pos;
        T_b_f_pos.resize(jointAngle.size());
        for(unsigned int i = 0; i < jointAngle.size(); i++)
        {
            robot->kdCalForwardPosition(jointAngle[i], true, false, T_b_f_pos[i]);
        }

        int ret = robot->calibToolDynamicParameterWithFTSensor(T_b_f_pos, mesureData, res);
        CHECK(ret >= 0);

        //5. 设置负载在法兰标系下的质量和质心，注意惯量不能辨识，赋值为 0
        res.inertia.inertia = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
        robot->mdlSetLoadDynamicParameterInFlange(res.inertia);

        //6. 得到在 tcp 处产生的外力
        interface::RLWrench Sensor_raw_data{-22.0025, 7.6498, -53.7775, -117.2313, -13.0462, 44.2201}; //传感器裸数据
        interface::RLWrench ft_sensor; // 去掉偏置后的数据
        for(unsigned int i = 0; i < Sensor_raw_data.size(); i++)
        {
            ft_sensor[i] = Sensor_raw_data[i] - res.offset[i];
        }

        interface::RLJntArray jnt = {0.6641, 0.3930, -1.1606, 0.0172, -1.5708, 0.6641};
        interface::RLJntArray jnt_zero = {0.0};

        interface::RLWrench RLWrench = robot->kdCalTCPWrenchFromEndSensor(ft_sensor, jnt, jnt_zero, jnt_zero); //
    }
}

参数设置说明

• 只补偿重力的情况下，工具和工件的惯性张量可以不设置，默认为0

• 只补偿重力的情况下，机械臂的速度，加速度均设为0

• 所有参数均采用国际标准单位制. 如距离(m) , 角度(rad), 质量(kg) 等